/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
//#include <mex.h>

/** gcc slam2D_linepose.c **/

/* mex -v slam2D_linepose.c */

#define DEG_TO_RAD 0.01745329251994
//#define zeros(X,Y) mxCreateDoubleMatrix(X,Y,mxREAL);
//#define sendup(X) mxSetName(X,#X); mexPutArray(X,"base")

double to2pi(double val) 
{
	while(val < -M_PI) 
	{
		val += 2*(double)M_PI;
	}
	
	while(val > M_PI) 
	{
		val -= 2*(double)M_PI;
	}
	return val;
}


int choldcb(double *R,double *A,unsigned  int N, unsigned int p)
{
    int i,j,k,l,lambda;
    double ir;
			
    for (j=0;j<N;j++)
    {
        lambda=j+p;
        if(lambda>N)
            lambda=N;
        lambda=lambda-j;

        for (i=0;i<lambda;i++)
            R[i+j*p]=A[i+j*p];

        k=j-p;
        if(k<0)
            k=0;	

        for (;k<j;k++)
        {
            l=k+p;	
            if(l>N)
                l=N;
            l=l-j;
            for (i=0;i<l;i++) R[i+j*p]=R[i+j*p]-R[i+(j-k)+k*p]*R[(j-k)+k*p];
        }
        if(R[j*p]<0)
            return 0;
    
        ir=1./sqrt(R[j*p]);
        for (i=0;i<lambda;i++) R[i+j*p]=R[i+j*p]*ir;
    }
    return 1;
}

void cholslb(double *A, double *b, double *x, int N, unsigned int p)
{
    int i,k,stop;
    double sum;
    
    for (i=0;i<N;i++)
    { 
        stop=i-p+1;
        if(stop<0)
            stop=0;
        for (sum=b[i],k=i-1;k>=stop;k--) sum -= A[(i-k)+k*p]*x[k];
        x[i]=sum/A[i*p]; /*BOOGIE*/
    }
    for (i=N-1;i>=0;i--)
    {
        stop=p;
        if(i+p>N)
            stop=N-i;
        for (sum=x[i],k=1;k<stop;k++) sum -= A[k+i*p]*x[i+k];
        x[i]=sum/A[i*p];/*BOOGIE*/
    }
}
/**
* Laskee mittauksen kovarianssin
*/
void calc2D_init_CC(
		double *CC,
		double *P,
		int frame,
		unsigned int N
)
{	
	unsigned int i;
	
	for(i=0;i<3;i++)
		CC[(i+frame*3)*N]+=P[i];
	
}
/**
* Päivittää mittauksen ristikovarianssimatriisia
**/
void calc2D_init_Ce(
		double *Ce,
		double *P,
		int frame,
		double *e
)
{
		Ce[frame*3]+=P[0]*e[0];
		Ce[frame*3+1]+=P[1]*e[1];
		Ce[frame*3+2]+=P[2]*e[2];

}

/**
* Päivittää odometriamittauksella kovarianssi matriisia
*/
void calc2D_link_CC(
		double *CC,
		double *S,
		unsigned int N,
		unsigned int frame
)
{	
	unsigned int base;
	base=frame*3-3;

/* e[0] 
	e[0]=X[3*frame]  -X[3*(frame-1)];
	e[1]=X[3*frame+1]-X[3*(frame-1)+1];
	e[2]=X[3*frame+2]-X[3*(frame-1)+2];
	
	*/
/* CC[i*N+j-i]=JJ(i,j)*/
	CC[(base)*N]+=S[0];
	CC[(3+base)*N]+=S[0];
	CC[3+base*N]+=-S[0];
/* e[1]*/ 	
	CC[(1+base)*N]+=S[1];
	CC[(4+base)*N]+=S[1];
	CC[3+(1+base)*N]+=-S[1];
	
	
/* e[2]*/ 	
	CC[(2+base)*N]+=S[2];
	CC[(5+base)*N]+=S[2];
	CC[3+(2+base)*N]+=-S[2];

}
/**
 * Päivittää odometriamittauksella ristikovarianssi matriisia
 */
void calc2D_link_Ce(
		double *Ce,
		double *S,
		double *e,
		unsigned int frame
)
{
	unsigned int base;
	/* x_frame-x_frame-1 */
	base=frame*3-3; 

	Ce[base]-=e[0]*S[0];
	Ce[base+1]-=e[1]*S[1];
	Ce[base+2]-=e[2]*S[2];
	Ce[base+3]+=e[0]*S[0];
	Ce[base+4]+=e[1]*S[1];
	Ce[base+5]+=e[2]*S[2];

}



/**
* Refines pose estimate based on odometry and weighted pose fixes 
* 
 * frames = num_frames*3 (num_frames = kaikkien pose pisteiden lkm)
 * @param  *P [frames x D] Internal coef - Harva kovarianssi... varaa tila
 * @param  *L [frames x D] - Harva cholesky esitys  - varaa tila
 * @param  *g [frames x 1] gradientti siirtymä - varaa muisti 
 * @param  *Xpos [frames x 1] Posteriori -       
 * @param  *Xpri [frames x 1] Priori - alkukäyrä
 * @param  *odo  [frames x 1] Differentiaalinen siirtymä vektori
 * @param  *base [N x 3] - Mittaukset poseista
 * @param  *frame [N * 1] - (int) indexit mittauksista
 * @param  *CC [frames x D] Internal coef - Harva kovarianssi... varaa tila - alusta nollaksi
 * @param  *Ce [Frames] Ristikovarianssi - alusta nollaksi
 * @param  *P0 [N x 3] Paino pose mittaukselle - suuri on iso paino eli niinku tosi hyvä mittaus
 * @param  *S  [3 x 1] Odometrian kohina (sama kaikille ny)
 * @param  *lamda [1x1] Levenberg marknad parametri - Alusta pieneksi  
 * @param  D Kovarianssimatriisin "bandwidth" - laita sellaseks neloseksi
 * @param  N Mittausten määrä
 * @param  num_frames kaikkien pose pisteiden lkm
 * @param  *er virhe ennen päivitttämistä
 **/
void update_pose(
		double *P,
		double *L,
		double *g,
		double *Xpos,
		double *Xpri,
		double *odo,
		double *base,
		int*frame,
		double *CC,
		double *Ce,
		double *P0,
		double *S,
		double *lambda,
		int D,
		int N,
		int num_frames,
		double *er
		)
{
	double e[3],v[3];
	int i,j,k,f,indi,ret;

	k=0;


  /**
	* Lasketaan virhe mittauksen ja priorin välillä
	* ja sitten päivitettään pose-vektorin varianssit CC ja Ce
	*/
	for(j=0;j<N;j++) /* for each measurement */
	{
			f=frame[j];
			v[0]=base[3*f];
			v[1]=base[3*f+1];
			v[2]=to2pi(base[3*f+2]);
			e[0]=-v[0]+Xpri[3*f];
			e[1]=-v[1]+Xpri[3*f+1];
			e[2]=-v[2]+Xpri[3*f+2];
			e[2]=to2pi(e[2]);
			er[k]=e[0];
			k++;
			er[k]=e[1];
			k++;
			er[k]=e[2];
			k++;
			calc2D_init_Ce(Ce,P0+j*3,f,e);
			calc2D_init_CC(CC,P0+j*3,f,D);
	}
	
	/**
	* Lasketaan virhe odometrian ja priorin välillä
	* ja päivitetään pose-vektorin varianssit CC ja Ce
	**/
	for(i=1;i<num_frames;i++) /* for each frame */
	{
		/// siirtymä error = (Xpri(i-1)+odo) - Xpri(i)  
		e[0]=-odo[(i-1)*3]-Xpri[(i-1)*3]+Xpri[i*3];
		e[1]=-odo[(i-1)*3+1]-Xpri[(i-1)*3+1]+Xpri[i*3+1];
		e[2]=-odo[(i-1)*3+2]-Xpri[(i-1)*3+2]+Xpri[i*3+2];
		e[2]=to2pi(e[2]);
		/*calculate covariance matrix multiplications */
		calc2D_link_Ce(Ce,S,e,i);
		calc2D_link_CC(CC,S,D,i);
		
		er[k]=e[0];
		k++;
		er[k]=e[1];
		k++;
		er[k]=e[2];
		k++;
	}

	/**
	* Ratkaistaan g = inv(CC)Ce
	* Levenberg-Marknad 
	**/
	do
	{
		for(j=0;j<D*num_frames*3;j++)
			P[j]=CC[j];
		for(j=0;j<num_frames*3;j++)
			P[j*D]+=(*lambda);
		ret=choldcb(L,P,num_frames*3,D);
		(*lambda)*=12.;
	}
	while(ret<0);

	cholslb(L,Ce,g,num_frames*3,D);
	for(j=0;j<num_frames*3;j++)Xpos[j]=Xpri[j]-g[j];
}

int main(void){
return 0;
}

#if 0

void proj2D_pose_linefun(
			double *e,
			double *J,
			double *v,
			double *pts,
			double *X,
			int ind
			  )
{
	double x,y,phi,num,dx,dy;
	double x1,x2,y1,y2,xp,yp,a,b,c,ang;
	double dedxp,dedyp,dxpdx,dxpdy,dxpdphi,dypdx,dypdy,dypdphi;
    
	
	
	x=v[0];
	y=v[1];
	phi=v[2];

	/*projecting object 2D-coordinates to vehicle frame*/
	x1=X[2*ind];
	y1=X[2*ind+1];
	x2=X[2*ind+2];
	y2=X[2*ind+3];
	dx=x1-x2;
	dy=y1-y2;/* hmm 2*ind or 4*ind */
	num=sqrt(dx*dx+dy*dy);
	a=dy/num;
	b=-dx/num;
	c=-a*x1-b*y1;
	ang=pts[1]+phi;
	xp=cos(ang)*pts[0]+x;
	yp=sin(ang)*pts[0]+y;
    
	/*residuals*/
	e[0]=a*xp+b*yp+c;
    
    /*BOOGIE? phi(a.k.a v[2]) is not restricted to range [-pi:pi]*/
    
    /*help variables ? necessary */
    
	dedxp=a;
	dedyp=b;
        
	dxpdphi=-sin(ang)*pts[0];
	dxpdx=1.;
	dxpdy=0.;
    
	dypdphi=cos(ang)*pts[0];
	dypdx=0.;
	dypdy=1.;
      
	/*Jacobian*/
	/*motion parameters*/
	J[0]=dedxp*dxpdx;
	J[1]=dedyp*dypdy;
	J[2]=dedxp*dxpdphi+dedyp*dypdphi;
	/*structure parameters */



}
void calc2D_pose_CC(
		double *CC,
		double *C,
		double R,
		unsigned int N,
		unsigned int frame)
{
	unsigned int base;
	
	base=frame*3; 
 
	CC[base*N]+=C[0]*C[0]*R;
	CC[1+base*N]+=C[0]*C[1]*R;
	CC[2+base*N]+=C[0]*C[2]*R;
	CC[base*N+N]+=C[1]*C[1]*R;
	CC[base*N+N+1]+=C[1]*C[2]*R;
	CC[base*N+2*N]+=C[2]*C[2]*R;
}

void calc2D_pose_Ce(
		double *Ce,
		double *C,
		double R,
		double *e,
		unsigned int frame)
{
	Ce[3*frame]+=C[0]*e[0]*R;
	Ce[3*frame+1]+=C[1]*e[0]*R;
	Ce[3*frame+2]+=C[2]*e[0]*R;
}

/******************************************************
void mexFunction(
				 int nlhs, mxArray *plhs[],
				 int nrhs, const mxArray *prhs[]
				 )
{
	const mxArray  *mz,*mXpri,*mindf,*mind,*mS,*mmap,*modo,*mP0,*mbase;
	mxArray	*mXpos,*me,*mC,*mCC,*mCe,*mP,*mL,*mg;	
	int i=0,D,N,num_frames;					 
	
	double *Xpos,*Xpri,*z,*u,*indf,*ind,*e,*C,*CC,*Ce,*P,*L,*g,*S,*odo,*base,*P0,*map,R,lambda;
	static int	expectRhs = 11;
	static int	expectLhs = 5;
	
	/* Check for proper number of arguments */
	
	if (nrhs != expectRhs) 
	{
		mexPrintf("slam2D_linepose requires %d input arguments. Got %d",
			expectRhs, nrhs);
	}
	else if (nlhs > expectLhs) 
	{
		mexPrintf("slam2D_linepose requires %d outputs arguments. Got %d",
			expectLhs, nlhs);
	}
	
	mXpri = prhs[i++];
	mz = prhs[i++];
	mmap = prhs[i++];
	modo = prhs[i++];
	mP0 = prhs[i++];
	mbase = prhs[i++];
	mS = prhs[i++];
	R=mxGetScalar(prhs[i++]);
	mindf = prhs[i++];
	mind = prhs[i++];
	lambda = mxGetScalar(prhs[i++]);
	
	D=mxGetNumberOfElements(mXpri);
	N=mxGetNumberOfElements(mind);
	num_frames=D/3;
	
	mXpos=zeros(D,1);
	me=zeros(1,N+D);
	mP=zeros(6,D);
	mL=zeros(6,D);
	mg=zeros(D,1);
	mCC=zeros(6,D);
	mCe=zeros(D,1);
	mC=zeros(4,N);
	S=mxGetPr(mS);
	map=mxGetPr(mmap);
	CC=mxGetPr(mCC);
	Ce=mxGetPr(mCe);
	C=mxGetPr(mC);
	Xpos=mxGetPr(mXpos);
	Xpri=mxGetPr(mXpri);
	base=mxGetPr(mbase);
	P0=mxGetPr(mP0);
	z=mxGetPr(mz);
	P=mxGetPr(mP);
	L=mxGetPr(mL);
	g=mxGetPr(mg);
	odo=mxGetPr(modo);
	indf=mxGetPr(mindf);
	ind=mxGetPr(mind);
	e=mxGetPr(me);

        slam2D_linepose(P,L,g,Xpos,Xpri,map,odo,base,indf,ind,z,CC,Ce,C,P0,S,R,&lambda,6,N,num_frames,e);
		
	i=0;
	
	plhs[i++]=mXpos;
	plhs[i++]=mCC;
	plhs[i++]=mCe;
	plhs[i++]=mC;
	plhs[i++]=me;

}
#endif
											
											